#include <math.h>
#include "qwt3d_color.h"
#include "qwt3d_plot.h"
#include "qwt3d_enrichment_std.h"
#include <QDebug>
using namespace Qwt3D;
/////////////////////////////////////////////////////////////////
//
//   Triangles
//
/////////////////////////////////////////////////////////////////
Triangles::Triangles()
{
  configure(10, 10, true);
  rgba.r=1;
  rgba.g=0;
  rgba.b=0;
  rgba.a=1;
}

Triangles::Triangles(double rad, double linewidth, bool smooth)
{
  configure(rad, linewidth, smooth);
}

void Triangles::configure(double rad, double linewidth, bool smooth)
{
  plot = 0;
  radius_ = rad;
  linewidth_ = linewidth;
  smooth_ = smooth;
}
/*void Triangles::configure(double rad,  bool smooth)
{
  plot = 0;
  radius_ = rad;
  linewidth_ = 5;
  smooth_ = smooth;
}*/
void Triangles::drawBegin()
{
  //  glLineWidth( 0 );
    glEnable(GL_POLYGON_OFFSET_FILL);
    //glPolygonOffset(1,1);
}

void Triangles::drawEnd()
{
  glEnd();
glDisable(GL_POLYGON_OFFSET_FILL);
}

void Triangles::draw()
{
    for(int i=0;i<lineData.size();i++)
    {
        draw(lineData[i]);
    }
}
void Triangles::add(const Triple &t)
{
    lineData.push_back(t);
}
void Triangles::draw(Qwt3D::Triple const& pos)
{
      glColor3d(rgba.r,rgba.g,rgba.b);
      glBegin(GL_TRIANGLES);
      glVertex3d(pos.x,pos.y+radius_*0.866,pos.z-radius_/3);//a
      glVertex3d(pos.x+0.75*radius_,pos.y-radius_*0.866/2,pos.z-radius_/3);//b
      glVertex3d(pos.x-0.75*radius_,pos.y-radius_*0.866/2,pos.z-radius_/3);//c
      glBegin(GL_TRIANGLES);
      glVertex3d(pos.x,pos.y+radius_*0.866,pos.z-radius_/3);//a
      glVertex3d(pos.x+0.75*radius_,pos.y-radius_*0.866/2,pos.z-radius_/3);//b
      glVertex3d(pos.x,pos.y,pos.z+radius_);//d
      glBegin(GL_TRIANGLES);
      glVertex3d(pos.x,pos.y+radius_*0.866,pos.z-radius_/3);//a
      glVertex3d(pos.x-0.75*radius_,pos.y-radius_*0.866/2,pos.z-radius_/3);//c
      glVertex3d(pos.x,pos.y,pos.z+radius_);//d
      glBegin(GL_TRIANGLES);
      glVertex3d(pos.x+0.75*radius_,pos.y-radius_*0.866/2,pos.z-radius_/3);//b
      glVertex3d(pos.x-0.75*radius_,pos.y-radius_*0.866/2,pos.z-radius_/3);//c
      glVertex3d(pos.x,pos.y,pos.z+radius_);//d
}

void Triangles::setLineColor(RGBA color)
{
    rgba=color;
}
void Triangles::clear()
{
    lineData.clear();
}
/////////////////////////////////////////////////////////////////
//
//   CrossHair
//
/////////////////////////////////////////////////////////////////

/*CrossHair::CrossHair()
{
  configure(10, 10, true, true);
  pos.x=5000;
  pos.y=0;
  pos.z=0;
}

CrossHair::CrossHair(double rad, double linewidth, bool smooth, bool boxed)
{
  configure(rad, linewidth, smooth, boxed);
}

void CrossHair::configure(double rad, double linewidth, bool smooth, bool boxed)
{
  plot = 0;
  radius_ = rad;
  linewidth_ = linewidth;
  smooth_ = smooth;
  boxed_ = boxed;
}
void CrossHair::configure(double rad, bool smooth)
{

}
void CrossHair::drawBegin()
{
  setDeviceLineWidth( linewidth_ );
  oldstate_ = glIsEnabled(GL_LINE_SMOOTH);
  if (smooth_)
    glEnable(GL_LINE_SMOOTH);
  else
    glDisable(GL_LINE_SMOOTH);
    glBegin( GL_LINE_STRIP );
}

void CrossHair::drawEnd()
{
  glEnd();

  if (oldstate_)
    glEnable(GL_LINE_SMOOTH);
  else
    glDisable(GL_LINE_SMOOTH);
}
void CrossHair::draw()
{
    draw(pos);
}
void CrossHair::draw(Qwt3D::Triple const& pos)
{
    //RGBA rgba = (*plot->dataColor())(pos);
    RGBA rgba(1,0.5,0.5,1);
	glColor4d(rgba.r,rgba.g,rgba.b,rgba.a);

  //double diag = (plot->hull().maxVertex-plot->hull().minVertex).length() * radius_;
double diag=6;
  glVertex3d( pos.x - diag, pos.y, pos.z); 
	glVertex3d( pos.x + diag, pos.y, pos.z); 
	
  glVertex3d( pos.x, pos.y - diag, pos.z); 
	glVertex3d( pos.x, pos.y + diag, pos.z); 
  
  glVertex3d( pos.x, pos.y, pos.z - diag); 
	glVertex3d( pos.x, pos.y, pos.z + diag); 

  // hull
  
  if (!boxed_)
    return;

  glVertex3d( pos.x - diag, pos.y - diag, pos.z + diag); 
	glVertex3d( pos.x + diag, pos.y - diag, pos.z + diag); 
  glVertex3d( pos.x - diag, pos.y - diag, pos.z - diag); 
	glVertex3d( pos.x + diag, pos.y - diag, pos.z - diag); 
  
  glVertex3d( pos.x - diag, pos.y + diag, pos.z + diag); 
	glVertex3d( pos.x + diag, pos.y + diag, pos.z + diag); 
  glVertex3d( pos.x - diag, pos.y + diag, pos.z - diag); 
	glVertex3d( pos.x + diag, pos.y + diag, pos.z - diag); 

  glVertex3d( pos.x - diag, pos.y - diag, pos.z + diag); 
	glVertex3d( pos.x - diag, pos.y + diag, pos.z + diag); 
  glVertex3d( pos.x - diag, pos.y - diag, pos.z - diag); 
	glVertex3d( pos.x - diag, pos.y + diag, pos.z - diag); 
  
  glVertex3d( pos.x + diag, pos.y - diag, pos.z + diag); 
	glVertex3d( pos.x + diag, pos.y + diag, pos.z + diag); 
  glVertex3d( pos.x + diag, pos.y - diag, pos.z - diag); 
	glVertex3d( pos.x + diag, pos.y + diag, pos.z - diag); 

  glVertex3d( pos.x - diag, pos.y - diag, pos.z - diag); 
	glVertex3d( pos.x - diag, pos.y - diag, pos.z + diag); 
  glVertex3d( pos.x + diag, pos.y - diag, pos.z - diag); 
	glVertex3d( pos.x + diag, pos.y - diag, pos.z + diag); 
  
  glVertex3d( pos.x - diag, pos.y + diag, pos.z - diag); 
	glVertex3d( pos.x - diag, pos.y + diag, pos.z + diag); 
  glVertex3d( pos.x + diag, pos.y + diag, pos.z - diag); 
	glVertex3d( pos.x + diag, pos.y + diag, pos.z + diag); 
}
void CrossHair::setLineColor(RGBA color)
{

}*/
/////////////////////////////////////////////////////////////////
//
//   Dot
//
/////////////////////////////////////////////////////////////////
/*
Dot::Dot()
{
  configure(1, false);
}

Dot::Dot(double pointsize, bool smooth)
{
  configure(pointsize, smooth);
}

void Dot::configure(double pointsize, bool smooth)
{
  plot = 0;
  pointsize_ = pointsize;
  smooth_ = smooth;
}

void Dot::drawBegin()
{
  setDevicePointSize( pointsize_ );
  oldstate_ = glIsEnabled(GL_POINT_SMOOTH);
  if (smooth_)
    glEnable(GL_POINT_SMOOTH);
  else
    glDisable(GL_POINT_SMOOTH);

  //glPointSize(10);
	glBegin( GL_POINTS );
}

void Dot::drawEnd()
{
  glEnd();

  if (oldstate_)
    glEnable(GL_POINT_SMOOTH);
  else
    glDisable(GL_POINT_SMOOTH);
}

void Dot::draw(Qwt3D::Triple const& pos)
{
	RGBA rgba = (*plot->dataColor())(pos);
  glColor4d(rgba.r,rgba.g,rgba.b,rgba.a);
  glVertex3d( pos.x, pos.y, pos.z);   
}
*/
Qwt3D::Dot::Dot()
{

    rgba.a = 1;
    rgba.b = 0.3;
    rgba.r = 0.6;
    rgba.g = 1;
}
Qwt3D::Dot::Dot(double thick,bool smooth)
{
    lineThick = thick;
    smooth_  = smooth;
    rgba.a = 1;
    rgba.b = 0.3;
    rgba.r = 0.6;
    rgba.g = 1;
}


void Qwt3D::Dot::configure(double thick, bool smooth)
{
    lineThick = thick;
    smooth_  = smooth;

}

void Qwt3D::Dot::drawBegin()
{

    setDevicePointSize(lineThick);

    oldstate_ = glIsEnabled(GL_POINT_SMOOTH);
    if (smooth_)
        glEnable(GL_POINT_SMOOTH);
    else
        glDisable(GL_POINT_SMOOTH);

    //glPointSize(10);
    glBegin( GL_POINTS);

}

void Qwt3D::Dot::drawEnd()
{
    glEnd();

    if (oldstate_)
        glEnable(GL_POINT_SMOOTH);
    else
        glDisable(GL_POINT_SMOOTH);
}

void Qwt3D::Dot::draw(Qwt3D::Triple const& pos)
{

    glColor4d(rgba.r,rgba.g,rgba.b,rgba.a);
    glVertex3d(pos.x,pos.y,pos.z);

}

void Qwt3D::Dot::draw()
{
    int i=0;
    for (i = 0; i < lineData.size(); i ++)
    {
        //RGBA rgba = getColor(i);
        //glColor4d(rgba.r,rgba.g,rgba.b,rgba.a);

        draw(lineData[i]);
    }

}
void Qwt3D::Dot::clear()
{
    lineData.clear();
}
void Qwt3D::Dot::add(Qwt3D::Triple const & t)
{
    lineData.push_back(t);
}
 void Qwt3D::Dot::setLineColor(RGBA color)
 {
    this->rgba = color;
 }

/////////////////////////////////////////////////////////////////
//
//   Cone
//
/////////////////////////////////////////////////////////////////

/*Cone::Cone()
{
	hat      = gluNewQuadric();
	disk     = gluNewQuadric();

  //configure(10, 3);
  rgba.a = 1;
  rgba.b = 0.3;
  rgba.r = 0.6;
  rgba.g = 1;
  pos.x=5000;
  pos.y=0;
  pos.z=0;
}

Cone::Cone(double rad, unsigned quality)
{
	hat      = gluNewQuadric();
	disk     = gluNewQuadric();

 // configure(rad, quality);
  rgba.a = 1;
  rgba.b = 0.3;
  rgba.r = 0.6;
  rgba.g = 1;
}

Cone::~Cone()
{
	gluDeleteQuadric(hat);
	gluDeleteQuadric(disk);
}

void Cone::configure(double rad, unsigned quality)
{
  plot = 0;
  radius_ = rad;
  quality_ = quality;
  oldstate_ = GL_FALSE;

	gluQuadricDrawStyle(hat,GLU_FILL);
	gluQuadricNormals(hat,GLU_SMOOTH);
	gluQuadricOrientation(hat,GLU_OUTSIDE);
	gluQuadricDrawStyle(disk,GLU_FILL);
	gluQuadricNormals(disk,GLU_SMOOTH);
	gluQuadricOrientation(disk,GLU_OUTSIDE);
}
void Cone::configure(double rad, bool quality)
{

}
void Cone::draw(Qwt3D::Triple const& pos)
{  
    //RGBA rgba = (*plot->dataColor())(pos);
  glColor4d(rgba.r,rgba.g,rgba.b,rgba.a);

  GLint mode;
	glGetIntegerv(GL_MATRIX_MODE, &mode);
	glMatrixMode( GL_MODELVIEW );
  glPushMatrix();

  glTranslatef(pos.x, pos.y, pos.z);

  gluCylinder(hat, 0.0, radius_, radius_*2, quality_, 1);
  glTranslatef(0, 0, radius_*2);
	gluDisk(disk, 0.0, radius_, quality_, 1);

  glPopMatrix();
	glMatrixMode(mode);
}
void Qwt3D::Cone::drawBegin()
{

}
void Qwt3D::Cone::drawEnd()
{
glEnd();
}
void Qwt3D::Cone::draw()
{
    draw(pos);
}
void Qwt3D::Cone::ChangePos(Triple pos_neo)
{
    pos=pos_neo;
}
void Qwt3D::Cone::setLineColor(RGBA color)
{
    rgba=color;
}*/
/////////////////////////////////////////////////////////////////
//
//   Arrow
//
/////////////////////////////////////////////////////////////////

Arrow::Arrow()
{	
	hat      = gluNewQuadric();
	disk     = gluNewQuadric();
	base    = gluNewQuadric();
	bottom   = gluNewQuadric();

	gluQuadricDrawStyle(hat,GLU_FILL);
	gluQuadricNormals(hat,GLU_SMOOTH);
	gluQuadricOrientation(hat,GLU_OUTSIDE);
	gluQuadricDrawStyle(disk,GLU_FILL);
	gluQuadricNormals(disk,GLU_SMOOTH);
	gluQuadricOrientation(disk,GLU_OUTSIDE);
	gluQuadricDrawStyle(base,GLU_FILL);
	gluQuadricNormals(base,GLU_SMOOTH);
	gluQuadricOrientation(base,GLU_OUTSIDE);
	gluQuadricDrawStyle(bottom,GLU_FILL);
	gluQuadricNormals(bottom,GLU_SMOOTH);
	gluQuadricOrientation(bottom,GLU_OUTSIDE);

	configure(3, 0.4, 0.06, 0.02);
}

Arrow::~Arrow()
{
	gluDeleteQuadric(hat);
	gluDeleteQuadric(disk);
	gluDeleteQuadric(base);
	gluDeleteQuadric(bottom);
}

/**
\param segs number of faces for the fields arrows (see the gallery for examples)
\param relconelength see picture
\param relconerad see picture
\param relstemrad see picture
\image html arrowanatomy.png 
*/
void Arrow::configure(int segs, double relconelength, double relconerad, double relstemrad)
{
	plot = 0;
  segments_ = segs;
  oldstate_ = GL_FALSE;
	rel_cone_length = relconelength;
	rel_cone_radius = relconerad;
	rel_stem_radius = relstemrad;	
}

void Arrow::draw(Qwt3D::Triple const& pos)
{	
	Triple end = top_;
	Triple beg = pos;
	Triple vdiff = end-beg;
	double length = vdiff.length();
	glColor4d(rgba_.r,rgba_.g,rgba_.b,rgba_.a);

	double radius[2];
	radius[0] = rel_cone_radius * length;
	radius[1] = rel_stem_radius * length;
	
	GLint mode;
	glGetIntegerv(GL_MATRIX_MODE, &mode);

	glMatrixMode( GL_MODELVIEW );
  glPushMatrix();


	Triple axis;
	double phi = calcRotation(axis, FreeVector(beg,end));
	
	glTranslatef(beg.x, beg.y, beg.z);
  glRotatef(phi, axis.x, axis.y, axis.z);

	double baseheight = (1-rel_cone_length) * length;
	
	glTranslatef(0, 0, baseheight);

	gluCylinder(hat, radius[0], 0.0, rel_cone_length * length, segments_,1);
	gluDisk(disk,radius[1],radius[0], segments_,1);
	
	glTranslatef(0, 0, -baseheight);
	
	gluCylinder(base, radius[1],radius[1], baseheight,segments_,1);
	gluDisk(disk,0,radius[1],segments_,1);

  glPopMatrix();
	glMatrixMode(mode);
}


//! transform a vector on the z axis with length |beg-end|, to get them in coincidence with the vector(beg,end)
/**
	\return Angle in degree to rotate
	\param axis   The axis to rotate around
	\param beg    result vector base point
	\param end    result vector top point
*/ 
double Arrow::calcRotation(Triple& axis, FreeVector const& vec)
{
	
	Triple end = vec.top;
	Triple beg = vec.base;

	Triple firstbeg(0.0,0.0,0.0);
	Triple firstend(0.0,0.0,(end-beg).length());
	
	Triple first = firstend - firstbeg;
	first.normalize();

	Triple second = end-beg;
	second.normalize();

	axis = normalizedcross(first,second);
	double cosphi = dotProduct(first,second);
	
	return 180 * acos(cosphi) / Qwt3D::PI;
}
void Arrow::configure(double linewidth, bool _smooth)
{

}
void Arrow::setLineColor(RGBA color)
{

}
void Arrow::add(const Triple &)
{

}
/////////////////////////////////////////////////////////////////
//
//   Line3D
//
/////////////////////////////////////////////////////////////////

Qwt3D::Line3D::Line3D()
{

    rgba.a = 1;
    rgba.b = 0.3;
    rgba.r = 0.6;
    rgba.g = 1;
}
Qwt3D::Line3D::Line3D(double thick,bool smooth)
{
	lineThick = thick;
	smooth_  = smooth;
    rgba.a = 1;
    rgba.b = 0.3;
    rgba.r = 0.6;
    rgba.g = 1;
}


void Qwt3D::Line3D::configure(double thick, bool smooth)
{
	lineThick = thick;
	smooth_  = smooth;
	
}

void Qwt3D::Line3D::drawBegin()
{
    setDeviceLineWidth(lineThick);

	oldstate_ = glIsEnabled(GL_LINE_SMOOTH);
	if (smooth_)
		glEnable(GL_LINE_SMOOTH);
	else
		glDisable(GL_LINE_SMOOTH);

    //glPointSize(10);
    glBegin( GL_LINE_STRIP);




}

void Qwt3D::Line3D::drawEnd()
{
    glEnd();

	if (oldstate_)
		glEnable(GL_LINE_SMOOTH);
	else
		glDisable(GL_LINE_SMOOTH);


}

void Qwt3D::Line3D::draw(Qwt3D::Triple const& pos)
{
	
    glColor4d(rgba.r,rgba.g,rgba.b,rgba.a);
	glVertex3d(pos.x,pos.y,pos.z);


}

void Qwt3D::Line3D::draw()
{
	for (int i = 0; i < lineData.size(); i ++)
	{
        //RGBA rgba = getColor(i);
        //glColor4d(rgba.r,rgba.g,rgba.b,rgba.a);

		draw(lineData[i]);
	}
}
void Qwt3D::Line3D::clear()
{
    lineData.clear();
}
void Qwt3D::Line3D::add(Qwt3D::Triple const & t)
{
	lineData.push_back(t);
}
 void Qwt3D::Line3D::setLineColor(RGBA color)
 {
    this->rgba = color;
 }

/*void Qwt3D::Line3D::setLineColor(int startIndex,RGBA color)
{
    //myColorMap[startIndex] = color;
}*/

/*Qwt3D::RGBA Qwt3D::Line3D::getColor(int pointIndex)
{
	Qwt3D::RGBA rgba(1,1,1,1);
	for (std::map<int,RGBA>::iterator it = myColorMap.begin(); it != myColorMap.end(); it ++)
	{
		if (it->first < pointIndex)
		{
			rgba = it->second;
			
		}
	}
	return rgba;
}*/

